#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_err.h>
#include <driver/gpio.h>
#include <driver/ledc.h>

#define SERVO_PIN 18
#define SERVO_MAX_ANGLE 180
#define SERVO_MIN_PULSEWIDTH 500
#define SERVO_MAX_PULSEWIDTH 2500

void servo_control_task(void *pvParameters)
{
    ledc_timer_config_t ledc_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT, // resolution of PWM duty
        .freq_hz = 50,                         // frequency of PWM signal
        .speed_mode = LEDC_LOW_SPEED_MODE,     // timer mode
        .timer_num = LEDC_TIMER_0,              // timer index
        .clk_cfg = LEDC_AUTO_CLK,               // Auto select the source clock
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel = {
        .channel = LEDC_CHANNEL_0,
        .duty = SERVO_MIN_PULSEWIDTH,
        .gpio_num = SERVO_PIN,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .hpoint = 0,
        .timer_sel = LEDC_TIMER_0
    };
    ledc_channel_config(&ledc_channel);

    while (1)
    {
        // Set servo to 0 degree angle
        ledc_set_duty(ledc_channel.speed_mode, ledc_channel.channel, SERVO_MIN_PULSEWIDTH);
        ledc_update_duty(ledc_channel.speed_mode, ledc_channel.channel);

        vTaskDelay(2000 / portTICK_PERIOD_MS);

        // Set servo to 180 degree angle
        ledc_set_duty(ledc_channel.speed_mode, ledc_channel.channel, SERVO_MAX_PULSEWIDTH);
        ledc_update_duty(ledc_channel.speed_mode, ledc_channel.channel);

        vTaskDelay(2000 / portTICK_PERIOD_MS);
    }
}

void dj_init()
{
    xTaskCreate(servo_control_task, "servo_control_task", 2048, NULL, 5, NULL);
}
